#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
import threading
from evdev import InputDevice, ecodes, list_devices

class PS5Controller(Node):
    def __init__(self):
        super().__init__('ps5_controller')
        self.publisher = self.create_publisher(Twist, '/cmd_vel', 10)
        self.twist = Twist()
        
        # 初始化游戏手柄
        self.device = self.find_ps5_controller()
        if not self.device:
            self.get_logger().error('PS5手柄未找到')
            return
            
        # 启动手柄读取线程
        self.running = True
        threading.Thread(target=self.read_controller).start()

    def find_ps5_controller(self):
        """查找PS5控制器"""
        for device in list_devices():
            dev = InputDevice(device)
            if "DualSense" in dev.name or "Wireless Controller" in dev.name:
                self.get_logger().info(f'找到PS5手柄: {dev.name}')
                return dev
        return None

    def read_controller(self):
        """读取PS5控制器输入"""
        center = 128.5
        deadzone = 3
        mapped_deadzone = 0.05  # 映射后的死区范围
        
        while self.running and self.device:
            try:
                for event in self.device.read_loop():
                    if event.type == ecodes.EV_ABS:
                        if event.code == ecodes.ABS_X:
                            # X轴：左正右负
                            raw_value = (event.value - center) / center
                            self.twist.angular.z = 0.0 if abs(raw_value) < mapped_deadzone else -raw_value
                        elif event.code == ecodes.ABS_Y:
                            # Y轴：前进为正
                            raw_value = (center - event.value) / center
                            self.twist.linear.x = 0.0 if abs(raw_value) < mapped_deadzone else raw_value
                        
                        self.get_logger().info(f"/cmd_vel - linear.x: {self.twist.linear.x:.2f}, angular.z: {self.twist.angular.z:.2f}")
                        self.publisher.publish(self.twist)

            except Exception as e:
                self.get_logger().error(f'读取手柄失败: {str(e)}')
                break

    def destroy_node(self):
        self.running = False
        if self.device:
            self.device.close()
        super().destroy_node()

def main():
    rclpy.init()
    controller = PS5Controller()
    try:
        rclpy.spin(controller)
    except KeyboardInterrupt:
        pass
    finally:
        controller.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()